/*
    The MIT License

    Copyright (c) 2010 IFMO/GameDev Studio

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in
    all copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
    THE SOFTWARE.
*/

#include "phys_local.h"

/*-----------------------------------------------------------------------------
	Physic entity :
-----------------------------------------------------------------------------*/

//
//
//
EPhysEntity::EPhysEntity( EPhysics *phys )
{
	this->phys	=	phys;
	
	trimesh		=	NULL;
	shape		=	NULL;
	body		=	NULL;
	mass		=	0;
	mesh		=	NULL;
}


//
//	EPhysEntity::~EPhysEntity
//
EPhysEntity::~EPhysEntity( void )
{
	DestructRigidBody();
}


/*-----------------------------------------------------------------------------
	Spawning :
-----------------------------------------------------------------------------*/

//
//	EPhysEntity::Spawn
//
void EPhysEntity::Spawn( void )
{
	ConstructRigidBody();
}


//
//	EPhysEntity::ConstructRigidBody
//
void EPhysEntity::ConstructRigidBody( void )
{
	//	destroy previously created body :	
	DestructRigidBody();

	if (type==PT_DYNAMIC) {	ConstructDynamic();	}	else 
	if (type==PT_STATIC	) {	ConstructStatic();	}	else
		RUNTIME_ERROR(va("Wrong EPhysEntity type %d", type));
}


//
//	EPhysEntity::cookTrimeshShape
//
btCollisionShape *EPhysEntity::cookTrimeshShape( const IPxTriMesh mesh )
{
	trimesh = new btTriangleMesh();

	btVector3    vertexPos[3];
	for (unsigned int n = 0; n < mesh->GetTriangleNum(); ++n)	{
	
		uint i0, i1, i2;
		mesh->GetTriangle(n, i0, i1, i2);
		
		EVertex v0	=	mesh->GetVertex(i0);
		EVertex v1	=	mesh->GetVertex(i1);
		EVertex v2	=	mesh->GetVertex(i2);
	
		vertexPos[0][0] = v0.position.x;
		vertexPos[0][1] = v0.position.y;
		vertexPos[0][2] = v0.position.z;

		vertexPos[1][0] = v1.position.x;
		vertexPos[1][1] = v1.position.y;
		vertexPos[1][2] = v1.position.z;

		vertexPos[2][0] = v2.position.x;
		vertexPos[2][1] = v2.position.y;
		vertexPos[2][2] = v2.position.z;

		trimesh->addTriangle(vertexPos[0], vertexPos[1], vertexPos[2]);
	}

	const bool useQuantizedAABB = true;
	
	btBvhTriangleMeshShape *shape = new btBvhTriangleMeshShape(trimesh, useQuantizedAABB);
	shape->setLocalScaling(btVector3(1,1,1));

	return shape;
}


//
//	EPhysEntity::cookConvexHull
//
btCollisionShape *EPhysEntity::cookConvexHull( const IPxTriMesh mesh )
{
	btConvexHullShape *hull	=	new btConvexHullShape();

	IPxTriMesh	temp	=	mesh->Clone();
	temp->SetFormat( GE_MESH_POSITION );
	temp->MergeVertices();

	for (uint i=0; i<temp->GetVertexNum(); i++) {
		EVertex	v = temp->GetVertex(i);
		hull->addPoint( ToPoint( v.position ) );
	}
	
	hull->setMargin(0);
	
	return hull;
}


//
//	EPhysEntity::ConstructDynamic
//
void EPhysEntity::ConstructDynamic( void )
{
	typedef btRigidBody::btRigidBodyConstructionInfo RBCI;

	//	create convex hull :
	shape	=	cookConvexHull( mesh );
	
	//	calculate inertia :
	//	TODO : Inertia tensor WTF???		
	//	WTF is solved here: http://code.google.com/p/bullet/issues/detail?id=413
	btVector3 inertia;
	shape->calculateLocalInertia(mass, inertia);

	//	fill construction info :		
	RBCI	ci (mass, this, shape, inertia);			  
	ci.m_startWorldTransform.setOrigin( ToPoint(position) );
	ci.m_startWorldTransform.setRotation( ToQuat(orient) );
	
	//	create and add body	:
	body	=	new btRigidBody(ci);

	phys->World()->addRigidBody( body );
	
	//	set RB simulating attributes :
	float	linear_damping			=	phys->phys_linear_damping->Float();
	float	angular_damping			=	phys->phys_angular_damping->Float();
	float	linear_sleep_treshold	=	phys->phys_linear_sleep_treshold->Float();
	float	angular_sleep_treshold	=	phys->phys_angular_sleep_treshold->Float();
	float	contact_proc_treshold	=	phys->phys_contact_proc_treshold->Float();
	
	body->setDamping					( linear_damping,		 angular_damping );
	body->setSleepingThresholds			( linear_sleep_treshold, angular_sleep_treshold );
	body->setContactProcessingThreshold	( contact_proc_treshold );
}


//
//	EPhysEntity::ConstructStatic
//
void EPhysEntity::ConstructStatic( void )
{
	typedef btRigidBody::btRigidBodyConstructionInfo RBCI;

	shape	=	cookTrimeshShape(mesh);
	
	//	construct rigid body :
	RBCI	ci(0, this, shape);
	ci.m_startWorldTransform.setOrigin( ToPoint(position) );
	ci.m_startWorldTransform.setRotation( ToQuat(orient) );
		
	body	=	new btRigidBody( ci );
	
	phys->World()->addRigidBody( body );
	
	body->setCollisionFlags( body->getCollisionFlags() | btCollisionObject::CF_STATIC_OBJECT );//STATIC_OBJECT);
	body->setContactProcessingThreshold(0);
}


//
//	EPhysEntity::DestructRigidBody
//
void EPhysEntity::DestructRigidBody( void )
{
	if (body) {
		phys->World()->removeRigidBody( body );
	}
	
	delete trimesh;	trimesh	=	NULL;
	delete shape;	shape	=	NULL;
	delete body;	body	=	NULL;
}


/*-----------------------------------------------------------------------------
	Attributes :
-----------------------------------------------------------------------------*/

//
//	EPhysEntity::SetMesh
//
void EPhysEntity::SetMesh( const IPxTriMesh mesh )
{
	this->mesh	=	mesh;
}


//
//	EPhysEntity::SetMass
//
void EPhysEntity::SetMass( float mass )
{
	this->mass	=	mass;
}


//
//	EPhysEntity::GetPose
//
void EPhysEntity::GetPose( EPoint &position, EQuaternion &orient ) const
{
	position	=	this->position;
	orient		=	this->orient;
}


//
//	EPhysEntity::SetPose
//
void EPhysEntity::SetPose( const EPoint &position, const EQuaternion &orient )
{
	this->position	=	position;
	this->orient	=	orient;
	
	if (body) {
		body->setCenterOfMassTransform( btTransform( ToQuat(orient), ToPoint(position) ) );
	}
}


/*-----------------------------------------------------------------------------
	Dynamics :
-----------------------------------------------------------------------------*/

//
//	EPhysEntity::ZeroForces
//
void EPhysEntity::ZeroForces( void )
{
	btVector3	f	=	body->getTotalForce();
	btVector3	t	=	body->getTotalTorque();
	
	body->applyTorque( -t );
	body->applyCentralForce( -f );
}


//
//	EPhysEntity::AddForce
//
void EPhysEntity::AddForce( const EVector &force )
{
	if (!body) return;
	body->applyCentralForce( ToVector( force ) );
}


//
//	EPhysEntity::AddForce
//
void EPhysEntity::AddTorque( const EVector &torque )
{
	if (!body) return;
	body->applyTorque( ToVector( torque ) );
}


//
//	EPhysEntity::AddForceAtPos
//
void EPhysEntity::AddForceAtPos( const EVector &force, const EPoint &point )
{
	if (!body) return;

	btTransform	t				=	body->getWorldTransform();

	btVector3	bt_force		=	ToVector( force );
	btVector3	bt_global_pos	=	ToPoint ( point );
	btVector3	bt_rel_pos		=	bt_global_pos - t.getOrigin();

	body->applyForce( bt_force, bt_rel_pos );
}

//
//	EPhysEntity::AddForceAtLocalPos
//
void EPhysEntity::AddForceAtLocalPos( const EVector &force, const EPoint &point )
{
	if (!body) return;
	
	btTransform	t				=	body->getWorldTransform();
	
	btVector3	bt_force		=	ToVector( force );
	btVector3	bt_local_pos	=	ToPoint ( point );
	btVector3	bt_rel_pos		=	t.getBasis() * bt_local_pos;
	
	body->applyForce( bt_force, bt_rel_pos );
}


/*-----------------------------------------------------------------------------
	Motion state :
-----------------------------------------------------------------------------*/

//
//	EPhysEntity::getWorldTransform
//
void EPhysEntity::getWorldTransform( btTransform& worldTrans ) const
{																		
	worldTrans.setRotation	( ToQuat( orient ) );
	worldTrans.setOrigin	( ToPoint( position ) );
}


//
//	EPhysEntity::setWorldTransform
//
void EPhysEntity::setWorldTransform( const btTransform& worldTrans )
{
	orient		=	ToQuat	( worldTrans.getRotation() );
	position	=	ToPoint ( worldTrans.getOrigin() );
}


//
//	EPhysEntity::GetLocalPointVelocity
//
EVector EPhysEntity::GetLocalPointVelocity( const EPoint &point ) const
{
	if (!body) return EVector::kZero;
	
	btTransform	t				=	body->getWorldTransform();
	
	btVector3	bt_local_pos	=	ToPoint( point );
	btVector3	bt_rel_pos		=	t.getBasis() * bt_local_pos;

	return ToVector( body->getVelocityInLocalPoint( bt_rel_pos ) );
}


//
//	EPhysEntity::GetCMOffset
//
EVector EPhysEntity::GetCMOffset( void ) const
{
	return EVector::kZero;
}


//
//	EPhysEntity::GetTotalForces
//
void EPhysEntity::GetTotalForces( EVector &force, EVector &torque ) const
{
	btVector3	tf	=	body->getTotalForce();
	btVector3	tm	=	body->getTotalTorque();
	
	force		=	ToVector( tf );
	torque		=	ToVector( tm );
}


//
//	EPhysEntity::GetMass
//
float EPhysEntity::GetMass( void ) const
{
	return 1.0f / body->getInvMass();
}

//
//	EPhysEntity::GetLinearVelocity
//
EVector	EPhysEntity::GetLinearVelocity() const
{
	btVector3 v = body->getLinearVelocity();
	return ToVector(v);
}

//
// EPhysEntity::SetLinearVelocity
//
void EPhysEntity::SetLinearVelocity(const EVector& velocity)
{
	body->setLinearVelocity(ToVector(velocity));
}

//
// EPhysEntity::GetAngularVelocity
//
EVector EPhysEntity::GetAngularVelocity() const
{
	btVector3 v = body->getAngularVelocity();
	return ToVector(v);
}

void EPhysEntity::SetInertia( const EVector &inertia )
{
	body->setInvInertiaDiagLocal( btVector3( 1.0 / inertia.x, 1.0 / inertia.y, 1.0 / inertia.z ) ); 
}


EVector EPhysEntity::GetInertia( void ) const
{
	btVector3 i = body->getInvInertiaDiagLocal();
	return EVector( 1.0 / i.x(), 1.0 / i.y(), 1.0 / i.z() );
}